#include <iostream>
#include <include.hpp>
#include <opencv2/opencv.hpp>
#include <cmath>
#include <fstream>
#include <filesystem>

double b, k_0, k_1;
double h_rk4;//rk的步长
double h_direct_traversal;//二分法求零点的步长
double rho_X_d, theta_X_d;
double rho_X_u, theta_X_u;
double theta_long_side;
cv::Point2d vec_e_long_side;
cv::Point2d X_d, X_u;
cv::Point2d vec_Xd_Xu;
double theta_diagonal;
double delta_theta_1;
double r;
cv::Point2d O_d, O_u;
cv::Point2d vec_Od_Ou;
double phi;
double k_q;
double theta_q;
double x_q, y_q;
cv::Point2d P_q;
cv::Point2d vec_Pq_Ou;
double delta_theta_2;
double t_0, t_1, t_2, t_3, t_4;

double theta[224] = {0};
double omega[224] = {0};
cv::RotatedRect rect[223];

cv::Mat img = cv::Mat::zeros(1000, 1000, CV_8UC3);
float k_offset = 40;
cv::Point2f b_offset = {500, 500};
// float k_offset = 4;
// cv::Point2f b_offset = {5, 5};

void pre_calculate(void)
{
    b = 1.7/2/M_PI;
    k_0 = 2.86;//这个是龙头
    k_1 = 1.65;
    h_rk4 = 0.05;
    h_direct_traversal = 0.001;

    t_0 = __DBL_MAX__;//先把进入圆的时间t_0设为很大

    rho_X_d = 4.5; theta_X_d = M_PI*90.0/17.0;
    rho_X_u = 4.5; theta_X_u = (64.0+5.0/17.0)*M_PI;

    theta_long_side = M_PI+atan2(theta_X_d*sin(theta_X_d)-cos(theta_X_d), sin(theta_X_d)+theta_X_d*cos(theta_X_d));//得到长方形长边的倾斜角度
    vec_e_long_side = cv::Point2d(cos(theta_long_side), sin(theta_long_side));
    // std::cout << theta_long_side * 180 / M_PI << std::endl;

    X_d = cv::Point2d(rho_X_d*cos(theta_X_d), rho_X_d*sin(theta_X_d));
    X_u = cv::Point2d(rho_X_u*cos(theta_X_u), rho_X_u*sin(theta_X_u));//先得到上下两个切点
    std::cout << "X_u: " << X_u << std::endl;

    vec_Xd_Xu = X_u - X_d;
    // std::cout << vec_d_u.x << vec_d_u.y << std::endl;
    theta_diagonal = atan2(vec_Xd_Xu.y, vec_Xd_Xu.x);//得到矩形对角线的倾斜角度
    // std::cout << theta_diagonal * 180 / M_PI << std::endl;

    delta_theta_1 = theta_diagonal - theta_long_side;//得到delta_theta

    r = 3/sqrt(2*(1-cos(M_PI-2*delta_theta_1)));
    std::cout << "r: " << r << std::endl;

    //接下来算两个圆的圆心
    O_d = X_d + 2 * r * vec_e_long_side;
    O_u = X_u - r * vec_e_long_side;
    std::cout << "O_d: " << O_d << ", O_u: " << O_u << std::endl;

    vec_Od_Ou = O_u - O_d;
    phi = M_PI-atan2(vec_Od_Ou.y, vec_Od_Ou.x);
    std::cout << "phi=" << phi * 180 / M_PI << std::endl;

    k_q = (O_u.x*O_u.y-r*sqrt(O_u.x*O_u.x + O_u.y*O_u.y-r*r)) / (O_u.x*O_u.x-r*r);
    std::cout << "k_q = " << k_q << std::endl;
    theta_q = atan(k_q);
    std::cout << "theta_q = " << theta_q * 180 / M_PI << std::endl;

    x_q = (O_u.x+k_q*O_u.y) / (1 + k_q*k_q);
    y_q = k_q*x_q;
    P_q = cv::Point2d(x_q, y_q);
    std::cout << "P_q: " << P_q << std::endl;

    vec_Pq_Ou = O_u - P_q;
    delta_theta_2 = acos(vec_Od_Ou.dot(vec_Pq_Ou) / (sqrt(vec_Od_Ou.x*vec_Od_Ou.x+vec_Od_Ou.y*vec_Od_Ou.y)*sqrt(vec_Pq_Ou.x*vec_Pq_Ou.x+vec_Pq_Ou.y*vec_Pq_Ou.y)));
    std::cout << "delta_theta_2 = " << delta_theta_2 * 180 / M_PI << std::endl;

}

// RK4方法，y0为初始条件，t0为初始时间，h为步长，steps为步数
double rk4(double (*f)(const double&, const double&), const double& y0, const double& t0, const double& h, const int& steps)
{
    double t = t0, y = y0;
    for (int i = 0; i < steps; ++i) {
        double k1 = h * f(y, t);
        double k2 = h * f(y + 0.5 * k1, t + 0.5 * h);
        double k3 = h * f(y + 0.5 * k2, t + 0.5 * h);
        double k4 = h * f(y + k3, t + h);
        y += (k1 + 2 * k2 + 2 * k3 + k4) / 6;
        t += h;
    }
    return y;
}

double dtheta_divided_by_dt_00(const double& theta, const double& t)
{
    //返回dtheta/dt
    return -1/(b*sqrt(1+theta*theta));
}

double dtheta_divided_by_dt_33(const double& theta, const double& t)
{
    return 1/(b*sqrt(1+pow(theta-59*M_PI, 2)));
}

double rho_theta(const double& theta)
{
    if (theta >= 4*M_PI+theta_q && theta < M_PI*73/17)
    {
        // std::cout << "1" << std::endl;
        // std::cout << "rho_theta: theta = " << theta << ", O_u.x = " << O_u.x << ", O_u.y = " << O_u.y << ", r = " << r << std::endl;
        // std::cout << "sqrt: " << r*r - pow(O_u.x*sin(theta) - O_u.y*cos(theta), 2) << std::endl;
        return O_u.x*cos(theta) + O_u.y*sin(theta) - sqrt((r*r - pow(O_u.x*sin(theta) - O_u.y*cos(theta), 2)));
    }
    else if (theta >= M_PI*73/17 && theta < M_PI*90/17)
    {
        // std::cout << "2" << std::endl;
        return O_d.x*cos(theta) + O_d.y*sin(theta) + sqrt(4*r*r - pow(O_d.x*sin(theta) - O_d.y*cos(theta), 2));
    }
    else if (theta >= M_PI*90/17 && theta <= 64*M_PI)
    {
        // std::cout << "3" << std::endl;
        return b*theta;
    }
    else if (theta >= 64*M_PI+theta_q && theta < (64.0+5.0/17.0)*M_PI)
    {
        // std::cout << "4" << std::endl;
        return O_u.x*cos(theta) + O_u.y*sin(theta) + sqrt(std::abs(r*r - pow(O_u.x*sin(theta) - O_u.y*cos(theta), 2)));
    }
    else if (theta >= (64.0+5.0/17.0)*M_PI)
    {
        // std::cout << "5" << std::endl;
        return -59*b*M_PI + b* theta;
    }
    else
    {
        std::cerr << theta << "不在 rho_theta 的定义域！" << std::endl;
        return -1;
    }
}
double f_theta(const double& theta)
{
    if (theta >= 4*M_PI+theta_q && theta <= M_PI*64.0)
    {
        return rho_theta(theta);
    }
    else if (theta < 4*M_PI+theta_q)
    {
        return rho_theta(-theta+68*M_PI+2*theta_q);
    }
    else
    {
        std::cerr << theta << "不在 f_theta 的定义域！" << std::endl;
        return -1;
    }
}

double target_g_theta(const double& theta_1, const double& theta_0, const double& k)
{
    double f_theta_1 = f_theta(theta_1);
    double f_theta_0 = f_theta(theta_0);
    return pow(f_theta_1, 2) + pow(f_theta_0, 2) - k*k - 2*f_theta_0*f_theta_1*cos(theta_1-theta_0);
}


double theta_0_t(const double& t, const double& theta_0_last, const double& k)
{
    double theta_0 = 0;
    if (t >= 0 && t < t_0)
    {
        // std::cout << "0" << std::endl;
        theta_0 = rk4(dtheta_divided_by_dt_00, theta_0_last, 0, h_rk4, 1);

        if (theta_0 <= 90.0*M_PI/17.0)
        {
            t_0 = t;
            t_1 = t_0 + 2*r*(M_PI - 2*delta_theta_1);
            t_2 = t_1 + delta_theta_2*r;
            t_3 = t_0 + 3*r*(M_PI - 2*delta_theta_1);
        }
    }
    else if (t >= t_0 && t < t_1)
    {
        // std::cout << "1" << std::endl;
        theta_0 = atan2(O_d.y + 2*r*sin(22.0*M_PI/17 - delta_theta_1 - 1.0/2/r*(t - t_0)), O_d.x + 2*r*cos(22.0*M_PI/17 - delta_theta_1 - 1.0/2/r*(t - t_0)));
        double mid = 0.5*(90.0+73.0)*M_PI/17;
        if (theta_0 < 73.0*M_PI/17)
        {       
            while (theta_0 < mid)
            {
                theta_0 += 2*M_PI;
            }
            if (std::abs(theta_0 - mid) > std::abs(theta_0 - 2*M_PI - mid))
            {
                theta_0 -= 2*M_PI;
            }
        }
        else if (theta_0 > 90.0*M_PI/17)
        {
            while (theta_0 > mid)
            {
                theta_0 -= 2*M_PI;
            }
            if (std::abs(theta_0 - mid) > std::abs(theta_0 + 2*M_PI - mid))
            {
                theta_0 += 2*M_PI;
            }
        }
    }
    else if (t >= t_1 && t < t_3)
    {
        
        double mid;
        if (t <= t_2)
        {
            theta_0 = atan2(O_u.y + r*sin(-phi + 1/r*(t - t_1)), O_u.x + r*cos(-phi + 1/r*(t - t_1)));
            // std::cout << "2" << std::endl;
            mid = 0.5*(4.0*M_PI+theta_q + 73.0*M_PI/17);
            if (theta_0 < 4.0*M_PI+theta_q)
            {
                while (theta_0 < mid)
                {
                    theta_0 += 2*M_PI;
                }
                if (std::abs(theta_0 - mid) > std::abs(theta_0 - 2*M_PI - mid))
                {
                    theta_0 -= 2*M_PI;
                }
            }
            else if (theta_0 > 73.0*M_PI/17)
            {
                while (theta_0 > mid)
                {
                    theta_0 -= 2*M_PI;
                }
                if (std::abs(theta_0 - mid) > std::abs(theta_0 + 2*M_PI - mid))
                {
                    theta_0 += 2*M_PI;
                }
            }
        }
        else
        {
            // std::cout << "3" << std::endl;
            theta_0 = atan2(O_u.y + r*sin(delta_theta_2-phi + 1/r*(t - t_2)), O_u.x + r*cos(delta_theta_2-phi + 1/r*(t - t_2)));
            mid = 0.5*(64*M_PI+theta_q + 64*M_PI+5.0/17.0*M_PI);
            if (theta_0 < 64*M_PI+theta_q)
            {
                while (theta_0 < mid)
                {
                    theta_0 += 2*M_PI;
                }
                if (std::abs(theta_0 - mid) > std::abs(theta_0 - 2*M_PI - mid))
                {
                    theta_0 -= 2*M_PI;
                }
            }
            else if (theta_0 > 64*M_PI+5.0/17.0*M_PI)
            {
                while (theta_0 > mid)
                {
                    theta_0 -= 2*M_PI;
                }
                if (std::abs(theta_0 - mid) > std::abs(theta_0 + 2*M_PI - mid))
                {
                    theta_0 += 2*M_PI;
                }
            }
        }
    }
    else if (t >= t_3)
    {
        // std::cout << "4" << std::endl;
        theta_0 = rk4(dtheta_divided_by_dt_33, theta_0_last, t_3, h_rk4, 1);
    }
    else
    {
        std::cerr << "不在 theta_t 的定义域！" << std::endl;
    }

    return theta_0;
}

double theta_1_theta_0(const double& theta_0, const double& k)
{
    double theta_0_cpy = theta_0;
    if (theta_0_cpy >= 64*M_PI+theta_q)
    {
        theta_0_cpy = -theta_0_cpy+68*M_PI+2*theta_q;
    }
    
    if (theta_0_cpy <= 64*M_PI)
    {
        double h = 0.1;
        double x_l = theta_0_cpy, x_r = x_l + h;
        while (target_g_theta(x_l, theta_0_cpy, k)*target_g_theta(x_r, theta_0_cpy, k) > 0)
        {
            x_l = x_r;
            x_r += h;
        }

        double mid = (x_l+x_r)/2;
        double epsilon = h_direct_traversal;
        double f_l, f_r, f_mid;
        while (std::abs(x_l-x_r) > epsilon)
        {
            f_l = target_g_theta(x_l, theta_0_cpy, k);
            f_mid = target_g_theta(mid, theta_0_cpy, k);
            f_r = target_g_theta(x_r, theta_0_cpy, k);
            if (0 == f_mid)
            {
                break;
            }
            else if (f_l*f_mid < 0)
            {
                x_r = mid;
            }
            else
            {
                x_l = mid;
            }
            mid = (x_l+x_r)/2;
        }

        if (mid < 4*M_PI+theta_q)
        {
            mid = -mid + 68*M_PI + 2*theta_q;
        }
        return mid;
    }
    else
    {
        std::cerr << "不在 theta_1_theta_0 的定义域！" << std::endl;
        return -1;
    }
}

void calculate(const double& end_time)
{
    std::ofstream result4("/home/zwh/myStudies/C++/model_4/result4.csv");
    std::ofstream resultv("/home/zwh/myStudies/C++/model_4/resultv.csv");

    int steps = static_cast<int>((end_time)/h_rk4);
    double t = 0;
    for (int i = 0; i < steps; ++i)
    // for (int i = 0; i < 1; ++i)
    {
        std::cout << "t_0: " << t_0 << std::endl;
        t = (i+1)*h_rk4;
        img = cv::Mat::zeros(1000, 1000, CV_8UC3);

        if (0 == i)
        {
            theta[0] = 16*M_PI;
        }
        else
        {
            theta[0] = theta_0_t(t, theta[0], k_0);
        }

        // omega[0] = -1/b/sqrt(1+theta[0]*theta[0]);//求得i*h时刻，第0个点的角速度

        theta[1] = theta_1_theta_0(theta[0], k_0);
        // omega[1] = omega_1(theta[0], theta[1], omega[0]);//第1个点的角速度
        // std::cout << "114" << std::endl;
        cv::Point2f X_0(rho_theta(theta[0])*cos(theta[0]), rho_theta(theta[0])*sin(theta[0]));
        cv::Point2f X_1(rho_theta(theta[1])*cos(theta[1]), rho_theta(theta[1])*sin(theta[1]));
        cv::Point2f vec_X = X_1 - X_0;
        cv::Point2f X_mid = (X_0 + X_1)/2;
        // std::cout << "X_0: " << X_0 << std::endl;
        // std::cout << "X_1: " << X_1 << std::endl;
        // std::cout << "X_mid: " << X_mid << std::endl;
        // std::cout << "vec_X: " << vec_X << std::endl;
        double angle_of_rect = (atan2(vec_X.y, vec_X.x)) * 180 / M_PI;//求得矩形的旋转角度（顺时针为正）
        // double angle_of_rect = 0;
        // std::cout << "angle_of_rect: " << angle_of_rect << std::endl;
        // rect[0] = cv::RotatedRect(cv::Point2f(250, 250), cv::Size2f(300, 100), 0);//求得的矩形即为龙头
        rect[0] = cv::RotatedRect(X_mid, cv::Size2f(3.41, 0.3), angle_of_rect);//求得的矩形即为龙头

        cv::Point2f head_vertices[4];
        rect[0].points(head_vertices);
        // std::cout << head_vertices[0] << ", " << head_vertices[1] << ", " << head_vertices[2] << ", " << head_vertices[3] << std::endl;
        std::array<cv::Point2f, 4> vec_head_vertices = {head_vertices[0], head_vertices[1], head_vertices[2], head_vertices[3]};
        // std::cout << "vec_head_vertices: " << std::endl;
        // std::cout << vec_head_vertices[0] << ", " << vec_head_vertices[1] << ", " << vec_head_vertices[2] << ", " << vec_head_vertices[3] << std::endl;
        for (int n = 0; n < 4; ++n)
        {
            // std::cout << "before: " << head_vertices[n];
            head_vertices[n] = k_offset * head_vertices[n];
            head_vertices[n].y *= -1;
            head_vertices[n] = head_vertices[n] + b_offset;
            // std::cout << ", after: " << head_vertices[n] << std::endl;
        }
        for (int n = 0; n < 4; ++n)
        {
            cv::line(img, head_vertices[n], head_vertices[(n + 1) % 4], cv::Scalar(0, 255, 0), 1);
        }


        for (int j = 2; j < 224; ++j)//求得(i+1)*h时刻后面所有点的角度
        {
            // std::cout << "128" << std::endl;
            theta[j] = theta_1_theta_0(theta[j-1], k_1);
            // omega[j] = omega_1(theta[j-1], theta[j], omega[j-1]);
            // std::cout << "theta[" << j << "]: " << theta[j] << std::endl;
            // std::cout << b*theta[j]*cos(theta[j]) << ", " << b*theta[j]*sin(theta[j]) << std::endl;
            cv::Point2f X_0(rho_theta(theta[j-1])*cos(theta[j-1]), rho_theta(theta[j-1])*sin(theta[j-1]));
            cv::Point2f X_1(rho_theta(theta[j])*cos(theta[j]), rho_theta(theta[j])*sin(theta[j]));
            cv::Point2f vec_X = X_1 - X_0;
            cv::Point2f X_mid = (X_0 + X_1)/2;
            double angle_of_rect = (atan2(vec_X.y, vec_X.x)) * 180 / M_PI;//求得矩形的旋转角度（顺时针为正）
            rect[j-1] = cv::RotatedRect(X_mid, cv::Size2f(2.2, 0.3), angle_of_rect);//求得第j-1个矩形
            // std::cout << "longtou: " << rect[0].center << std::endl;
            // std::cout << rect[j-1].center << std::endl;
            // std::cout << "135" << std::endl;
            //接下来判断龙头的四个点是否在后面若干个矩形的内部
            cv::Point2f vertices[4];
            rect[j-1].points(vertices);
            std::array<cv::Point2f, 4> vec_vertices = {vertices[0], vertices[1], vertices[2], vertices[3]};
            for (int n = 0; n < 4; ++n)
            {
                vertices[n] = k_offset * vertices[n];
                vertices[n].y *= -1;
                vertices[n] = vertices[n] + b_offset;
            }
            for (int n = 0; n < 4; ++n)
            {
                cv::line(img, vertices[n], vertices[(n + 1) % 4], cv::Scalar(255, 0, 0), 1);
            }


            // for (int n = 0; n < 4; ++n)
            // {
            //     double dis = cv::pointPolygonTest(vec_vertices, vec_head_vertices[n], true);
                
            //     // std::cout << vec_vertices[n] << ", ";
            //     // std::cout << "flg: " << flg << "; ";
            //     if (j != 2 && dis >= 0)
            //     {
            //         std::cout << "在第" << t << "s时碰撞！" << std::endl;
            //         // flg_to_end = true;
            //     }
            // }
        }

        if (i >= 6091 && i <= 6091+20*200)
        {
            if ((i-6091) % 20 == 0)
            {
                for (int n = 0; n < 224; ++n)
                {
                    result4 << rho_theta(theta[n])*cos(theta[n]) << "," << rho_theta(theta[n])*sin(theta[n]);
                    if (n < 223)
                    {
                        result4 << ",";
                    }
                    else
                    {
                        result4 << '\n';
                    }
                }
            }
        }
        // for (int n = 0; n < 224; ++n)
        // {
        //     std::cout << "theta[" << n << "]: " << theta[n] << ", rho_theta = " << rho_theta(theta[n]) << std::endl;
        // }
        cv::imshow("img", img);
        cv::waitKey(1);
    }
    result4.close();
    resultv.close();

}

int main(void)
{
    pre_calculate();
    calculate(1000);

    // int steps = static_cast<int>(2*M_PI/0.1);
    // for (int i = 0; i < steps; ++i)
    // {
    //     std::cout << "theta: " << 64*M_PI+theta_q+0.1*i << ", rho: " << rho_theta(64*M_PI+theta_q+0.1*i) << std::endl;
    // }
    return 0;
}